#include "usrRemote.h"
#include <stdio.h>
#include "zucmotcfg.h"
#include "rtapi/rtapi_math.h"
// #define ZUCMOT_MAX_AXIS 9
simxInt vrep_clientID = -1;
simxInt vrep_connectOK = 0;
simxInt Vrep_Active = -1;

const double DEG2RAD = 3.1415926 / 180.0;

simxInt JointHandles[ZUCMOT_MAX_AXIS] = {0};
const char SIGNAL_NAME_CONVEYOR_VEL[255] = "conveyorVel";

simxInt InitVrepClient()
{
    if (Vrep_Active < 0)
    {
        return simx_return_ok;
    }
    vrep_clientID = simxStart(VREP_IP, VREP_PORT, 1, 1, 3000, 5);
    printf("vrep_clientID %d\n", vrep_clientID);
    if (vrep_clientID < 0)
    {
        return vrep_clientID;
    }
    simxInt jointcount = 0;
    simxInt* pJointhandle = JointHandles;
    // printf("init pJointhandle 0x%x, 0x%x\n", pJointhandle, JointHandles);
    simxInt errId = simxGetObjects(vrep_clientID, sim_object_joint_type, &jointcount, &pJointhandle, simx_opmode_blocking);
    if (errId == simx_return_ok)
    {
        vrep_connectOK = 1;
    }
    else
    {
        vrep_connectOK = 0;
    }
    for (int i = 0; i < jointcount; i++) { JointHandles[i] = *(pJointhandle + i); }
    // printf("init pJointhandle 0x%x, 0x%x\n",  pJointhandle, JointHandles);
    // printf("errID %d, Jointcount %d, JointHandles %d, %d, %d, %d, %d, %d\n", errId, jointcount, *pJointhandle, *(pJointhandle+1), *(pJointhandle+2), *(pJointhandle)+3, *(pJointhandle+4), *(pJointhandle+5));
    // printf("pJointhandle 0x%x, 0x%x, 0x%x, 0x%x\n",  (*pJointhandle), (*pJointhandle)+1, (*pJointhandle)+2, (*pJointhandle)+3);
    // printf("JointHandles 0x%x, 0x%x, 0x%x, 0x%x, 0x%x\n",  JointHandles, &JointHandles[0], &JointHandles[1], &JointHandles[2], &JointHandles[3]);
    // printf("JointHandles %d, %d, %d, %d, %d, %d\n", JointHandles[0], JointHandles[1], JointHandles[2], JointHandles[3], JointHandles[4], JointHandles[5]);

    simxStartSimulation(vrep_clientID, simx_opmode_oneshot);
    simxSynchronous(vrep_clientID, 1);
    simxSynchronousTrigger(vrep_clientID);
    return errId;
}

void closeVrepSimulator()
{
    if (vrep_clientID < 0)
    {
        return;
    }
    if (simxGetConnectionId(vrep_clientID) < 0)
    {
        return;
    }

    //  =================== buffer ==============
    // simxFloat tmp;
    // for (int i = 0; i < ZUCMOT_MAX_AXIS; i++){
    //     if (JointHandles[i] > 0){
    //         simxGetJointPosition(vrep_clientID, JointHandles[i], &tmp, simx_opmode_oneshot);
    //     }
    // }
    // simxInt pingTime;
    // simxGetPingTime(vrep_clientID, &pingTime);
    simxStopSimulation(vrep_clientID, simx_opmode_blocking);

    simxFinish(vrep_clientID);
}

simxInt swapJointPosData(const double* poscmd, double* posfb)
{
    if (vrep_clientID < 0)
    {
        return vrep_clientID;
    }
    simxInt errId = simx_return_ok;
    // simxPauseCommunication(vrep_clientID, 1);
    simxFloat tmpValue = 0;
    // uint64_t start = rtapi_get_time() ;
    for (int i = 1; i < ZUCMOT_MAX_AXIS; i++)
    {
        if (JointHandles[i] > 0)
        {
            // errId = simxSetJointTargetPosition(vrep_clientID, JointHandles[i], (simxFloat)poscmd[i]*DEG2RAD, simx_opmode_oneshot);
            errId = simxSetJointPosition(vrep_clientID, JointHandles[i], (simxFloat)poscmd[i] * DEG2RAD, simx_opmode_oneshot);
        }
    }
    // rtapi_print("cmdPos %f, %f, %f, %f, %f, %f \n", poscmd[0], poscmd[1], poscmd[2], poscmd[3],poscmd[4], poscmd[5]);
    // printf("cmdPos %f, %f, %f, %f, %f, %f \n", poscmd[0], poscmd[1], poscmd[2], poscmd[3],poscmd[4], poscmd[5]);
    // simxPauseCommunication(vrep_clientID, 0);

    // for (int i = 0; i < ZUCMOT_MAX_AXIS; i++)
    // {
    //     if (JointHandles[i] > 0)
    //     {
    //         errId = simxGetJointPosition(vrep_clientID, JointHandles[i], &tmpValue, simx_opmode_oneshot);
    //         posfb[i] = tmpValue;
    //     }
    // }
    simxSynchronousTrigger(vrep_clientID);

    // simxInt pingTime;
    // simxGetPingTime(vrep_clientID, &pingTime);

    return errId;
}

simxInt swapJointTrqData(const double* trqcmd, double* trqfb)
{
    if (simxGetConnectionId(vrep_clientID) < 0 || !vrep_connectOK)
    {
        return vrep_clientID;
    }
    simxInt errId = simx_return_ok;
    //simxPauseCommunication(vrep_clientID, 1);
    simxFloat tmpValue = 0;
    for (int i = 0; i < ZUCMOT_MAX_AXIS; i++)
    {
        if (JointHandles[i] > 0)
        {
            errId = simxSetJointMaxForce(vrep_clientID, JointHandles[i], (simxFloat)trqcmd[i], simx_opmode_oneshot);
            errId = simxGetJointForce(vrep_clientID, JointHandles[i], &tmpValue, simx_opmode_oneshot);
            trqfb[i] = tmpValue;
        }
    }
    //simxPauseCommunication(vrep_clientID, 0);
    return errId;
}

simxInt swapJointVelData(const double* velcmd, double* velfb)
{
    if (simxGetConnectionId(vrep_clientID) < 0 || !vrep_connectOK)
    {
        return vrep_clientID;
    }
    simxInt errId = simx_return_ok;
    //simxPauseCommunication(vrep_clientID, 1);
    for (int i = 0; i < ZUCMOT_MAX_AXIS; i++)
    {
        if (JointHandles[i] > 0)
        {
            errId = simxSetJointTargetVelocity(vrep_clientID, JointHandles[i], (simxFloat)velcmd[i], simx_opmode_oneshot);
            //errId = simxGetJoint(vrep_clientID, JointHandles[i], &velfb[i], simx_opmode_blocking);
        }
    }
    //simxPauseCommunication(vrep_clientID, 0);
    return errId;
}

simxInt ReadConveyorPos(double* velfb)
{
    if (simxGetConnectionId(vrep_clientID) < 0 || !vrep_connectOK)
    {
        return vrep_clientID;
    }
    simxFloat tmpVal = 0;
    simxInt errId = simxGetFloatSignal(vrep_clientID, SIGNAL_NAME_CONVEYOR_VEL, &tmpVal, simx_opmode_oneshot);
    *velfb = tmpVal;
    return errId;
}